import java.util.*;
import java.io.*;

class MoCrapWarp
{
   public static void main(String[] args)
   {
       boolean doWholeFile = false;
       int motionIndex = 0;
       int xyz = 0;
       MoCrapWarp program = new MoCrapWarp();
       if (args.length == 4)
       {
           motionIndex = Integer.parseInt(args[3]);
       }
       else
       {
           if (args.length == 5)
           {
               if (args[4].equals("x"))
                   xyz = 0;
               if (args[4].equals("y"))
                   xyz = 1;
               if (args[4].equals("z"))
                   xyz = 2;
               motionIndex = Integer.parseInt(args[3]);
           }
           else
           {
               if (args.length != 3)
               {
                   program.fail("Wrong number of arguments.");
               }
               else
               {
                   doWholeFile = true;
               }
           }
       }

       AMCParser amcParser = new AMCParser();
       ASFParser asfParser = new ASFParser();
       asfParser.parseASF(args[0]);
       ArrayList<Bone> skeleton = asfParser.skeleton;
       ArrayList<Motion> motion = amcParser.parseFile(args[1]);
       for (int i = 0; i < motion.size(); i++)
       {
           String s = motion.get(i).name();
           int n = asfParser.searchBone(s, skeleton);
           motion.get(i).setLimits(skeleton.get(n).getLimits());
       }
       ////motion = program.process(motion); //does band equalizing

       ////for (int i = 0; i < motion.size(); i++)
       ////    motion.get(i).updateLimits();

       ////amcParser.outputFile(motion, args[2]); //outputs new amc

       //------------------------ BELOW HERE IS TIMEWARPING STUFF ---

       ArrayList<Motion> motion2 = amcParser.parseFile(args[2]);

       if (doWholeFile == false)
       {
           double[] w1 = motion.get(motionIndex).getWave(xyz);
           double[] w2 = motion2.get(motionIndex).getWave(xyz);
    
           MatLabArray m1 = new MatLabArray("motionA",w1,
            Math.min(w1.length,w2.length));
           MatLabArray m2 = new MatLabArray("motionB",w2,
            Math.min(w1.length,w2.length));
    
           //m1.toFile("m1.array");
           //m2.toFile("m2.array");
    
           TimeWarp myWarp = new TimeWarp(w1,w2);
    
           double[] warped = myWarp.getWave(); // this contains the warped data
                                               // to do with what you will
    
           MatLabArray m3 = new MatLabArray("warped",warped);
    
          // m3.toFile("warped.array");
    
          new MotionResult("results.m",m1,m2,m3,myWarp.costMatrix,myWarp.pathArray);
    
           //for (int i = 0; i < motion.size(); i++)
           //    motion.get(i).updateLimits();
       }
       else
       {
           System.out.println("WARNING : WARPING ALL MOTION DATA...");
           ArrayList<Motion> outputMotions = motion;
           Motion[] outMotions = new Motion[outputMotions.size()];

           for (int i = 0; i < outputMotions.size(); i++)
           {
               Motion warpA = motion.get(i);
               System.out.println("    Warping motion for bone '"+warpA.name()+"'...");
               Motion warpB = amcParser.getMotion(outputMotions.get(i).name(),motion2);
               outMotions[i] = new Motion(warpA);

               if ((warpA != null) && (warpB != null))
               {
                   int warpCount = Math.min(warpA.numWaves(),warpB.numWaves());
                   int totalWCount = Math.max(warpA.numWaves(),warpB.numWaves());
                   if (warpA.numWaves() != warpB.numWaves())
                       System.out.println("        * Non-matched signals discarded *");

                   boolean first = true;

                   for (int j = 0; j < warpCount; j++)
                   {
                       System.out.println("        Warping on signal "+j+"...");
                       TimeWarp aWarp = new TimeWarp(warpA.getWave(j),warpB.getWave(j));
                       double[] warpDat = aWarp.getWave(); // this contains the warped data
                       if (first)
                       {
                           outMotions[i].initWaves(totalWCount,warpDat.length);
                           first = false;
                       }
                       outMotions[i].addWave(j,warpDat);
                       System.out.println("        Completed warping on signal.");
                   }
                   outputMotions.set(i,outMotions[i]);
               }
               else
               {
                   System.out.println("    * Non-matched bone discarded *");
               }
               System.out.println("Completed bone motion data warp.");
           }

           amcParser.outputFile(outputMotions, "alignedAndReady.amc"); //outputs new amc
       }
   }

   ArrayList<Motion> process(ArrayList<Motion> motion)
   {
       for (int i = 1; i < motion.size(); i++)
       {
           motion.get(i).createPyramids();
           int s = motion.get(i).numWaves();
           int x = motion.get(i).laplacianSize();
           //x = (int)(x/2);
           for (int j = 0; j < s; j++)
               motion.get(i).multiplyLaplacian(j, x-1, 50);
           motion.get(i).reconstruct();
       }
       return motion;
   }
   
   void fail(String msg)
   {
       System.err.println("Error: " + msg);
       System.exit(1);
   }
}
